﻿#include "TemplateMatch.h"
#include <iostream>
#include <limits>
#include <QDebug>
#include <opencv2/highgui.hpp>
#include "qtOpenCV/ImageRotate.h"

namespace Qly {

inline void removeInvalidData(cv::Mat &result)
{
    for(int i = 0; i < result.rows; i++)
    {
         float* pLine = result.ptr<float>(i);
         for(int j = 0; j < result.cols; j++)
         {
             if(isinf(pLine[j]) || isnan(pLine[j]))
             {
                 pLine[j] = 0.0;
             }
         }
    }
}

inline void removeBigData(cv::Mat &result)
{
    for(int i = 0; i < result.rows; i++)
    {
         float* pLine = result.ptr<float>(i);
         for(int j = 0; j < result.cols; j++)
         {
             if(pLine[j] > 1e11)
             {
                 pLine[j] = 0.0;
             }
         }
    }
}

/**
 * @brief templateMatchRaw 最最原始的模板匹配方法
 * @param image 输入图像
 * @param tepl 模板图像
 * @param method 匹配方法，具体参见 cv::matchTemplate() 函数
 * @param point 返回匹配位置
 * @return 返回匹配分数，DIFF 类匹配数值越小表示匹配越好。COFF 类匹配数值越大越好。
 */
double templateMatch(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask, int method, cv::Point &point) noexcept
{
    int result_cols = image.cols - tepl.cols + 1;
    int result_rows = image.rows - tepl.rows + 1;
    cv::Mat result = cv::Mat( result_cols, result_rows, CV_32FC1 );
    cv::matchTemplate( image, tepl, result, method, mask );
    if(method == cv::TM_CCOEFF_NORMED) removeInvalidData(result);
    double minVal, maxVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat() );
    switch(method)
    {
    case cv::TM_SQDIFF:
    case cv::TM_SQDIFF_NORMED:
        point = minLoc;
        return minVal;

    case cv::TM_CCORR:
    case cv::TM_CCOEFF:
    case cv::TM_CCORR_NORMED:
    case cv::TM_CCOEFF_NORMED:
        point = maxLoc;
        return maxVal;

    default: point = maxLoc;
        return maxVal;
    }
}

bool templateMatch(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask, int method,
                   cv::Point &point, double &score) noexcept
{
    if(image.empty() || tepl.empty() ) return false;
    if(method < 0 || method > 6) return false;
    cv::Mat result;// = cv::Mat( result_cols, result_rows, CV_32FC1 );
    cv::matchTemplate( image, tepl, result, method, mask );
    if(method == cv::TM_CCOEFF_NORMED) removeInvalidData(result);
    double minVal, maxVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat() );
    switch(method)
    {
    case cv::TM_SQDIFF:
    case cv::TM_SQDIFF_NORMED:
        point = minLoc;
        score = minVal;
        break;
    case cv::TM_CCORR:
    case cv::TM_CCOEFF:
    case cv::TM_CCORR_NORMED:
    case cv::TM_CCOEFF_NORMED:
        point = maxLoc;
        score = maxVal;
        break;
    default: point = maxLoc;
        score = maxVal;
        break;
    }
    return true;
}

bool templateMatch(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask, int method,
                   cv::Rect &rect, double &score) noexcept
{
    if(image.empty() || tepl.empty() ) return false;
    if(method < 0 || method > 6) return false;
    cv::Mat result;// = cv::Mat( result_cols, result_rows, CV_32FC1 );
    cv::matchTemplate( image, tepl, result, method, mask );
    if(method == cv::TM_CCOEFF_NORMED || method == cv::TM_CCORR_NORMED) removeInvalidData(result);
    //if(method == cv::TM_CCORR) removeBigData(result);
    double minVal, maxVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat() );
    switch(method)
    {
    case cv::TM_SQDIFF:
    case cv::TM_SQDIFF_NORMED:
        rect = cv::Rect(minLoc, tepl.size());
        score = minVal;
        break;
    case cv::TM_CCORR:
    case cv::TM_CCOEFF:
    case cv::TM_CCORR_NORMED:
    case cv::TM_CCOEFF_NORMED:
        rect = cv::Rect(maxLoc, tepl.size());
        score = maxVal;
        break;
    default:
        rect = cv::Rect(maxLoc, tepl.size());
        score = maxVal;
        break;
    }
    return true;
}

bool templateMatchCCORR(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask,
                        cv::Point &point, double &score) noexcept
{
    if(image.empty() || tepl.empty() ) return false;
    cv::Mat result;
    cv::matchTemplate( image, tepl, result, cv::TM_CCORR, mask );
    double minVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &score, &minLoc, &maxLoc, cv::Mat() );
    point = maxLoc;
    return true;
}

bool templateMatchCCORR_NORMED(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask,
                               cv::Point &point, double &score) noexcept
{
    if(image.empty() || tepl.empty() ) return false;
    cv::Mat result;
    cv::matchTemplate( image, tepl, result, cv::TM_CCORR_NORMED, mask );
    double minVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &score, &minLoc, &maxLoc, cv::Mat() );
    point = maxLoc;
    return true;
}

bool templateMatchCCORR(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask,
                        cv::Rect &rect, double &score) noexcept
{
    if(image.empty() || tepl.empty() ) return false;
    cv::Mat result;
    cv::matchTemplate( image, tepl, result, cv::TM_CCORR, mask );
    double minVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &score, &minLoc, &maxLoc, cv::Mat() );
    rect = cv::Rect(maxLoc, tepl.size());
    return true;
}

bool templateMatchCCORR_NORMED(const cv::Mat &image, const cv::Mat &tepl, const cv::Mat &mask,
                               cv::Rect &rect, double &score) noexcept
{
    if(image.empty() || tepl.empty() ) return false;
    cv::Mat result;
    cv::matchTemplate( image, tepl, result, cv::TM_CCORR_NORMED, mask );
    double minVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &score, &minLoc, &maxLoc, cv::Mat() );
    rect = cv::Rect(maxLoc, tepl.size());
    return true;
}


bool templateMatchCCOEFF_NORMED(const cv::Mat &image, const cv::Mat &model, const cv::Mat &mask,
                                cv::Point &point, double &score) noexcept
{
    if(image.empty()) return false;
    if(model.empty()) return false;
    if(!mask.empty() && mask.size() != model.size()) return false;
    if(image.rows < model.rows) return false;
    if(image.cols < model.cols) return false;

    double minVal = 1, maxVal = -1;
    cv::Point minLoc, maxLoc;
    cv::Mat result;
    cv::matchTemplate(image, model, result, cv::TM_CCOEFF_NORMED, mask);

    removeInvalidData(result);
    cv::minMaxLoc(result,  &minVal, &maxVal, &minLoc, &maxLoc);
    score = maxVal;
    point = maxLoc;
    return true;

}

bool templateMatchCCOEFF_NORMED(const cv::Mat &image, const cv::Mat &model, const cv::Mat &mask,
                                cv::Rect &rect, double &score) noexcept
{
    if(image.empty()) return false;
    if(model.empty()) return false;
    if(!mask.empty() && mask.size() != model.size()) return false;
    if(image.rows < model.rows) return false;
    if(image.cols < model.cols) return false;

    double minVal = 1, maxVal = -1;
    cv::Point minLoc, maxLoc;
    cv::Mat result;
    cv::matchTemplate(image, model, result, cv::TM_CCOEFF_NORMED, mask);

    removeInvalidData(result);
    cv::minMaxLoc(result,  &minVal, &maxVal, &minLoc, &maxLoc);
    score = maxVal;

    rect = cv::Rect(maxLoc, model.size());
    return true;
}


bool TemplateMatch::better(double oldGrade, double newGrade, int method)
{
    switch (method)
    {
    case cv::TM_SQDIFF:
    case cv::TM_SQDIFF_NORMED:
        if(newGrade < oldGrade)
        {
            return true;
        }
         break;
    default:
        if(newGrade > oldGrade)
        {
            return true;
        }
    }
    return false;
}

bool TemplateMatch::angleInterpolate(size_t index, double &angle, double &score)
{
    if(index >= m_angleCount) return false;
    if(index == 0)
    {
        angle = m_angles[0];
        score = m_scores[0];
        return true;
    }
    if(index == m_angleCount - 1)
    {
        angle = m_angles[index];
        score = m_angles[index];
        return true;
    }
    double a, b, c; //二次函数
    c = m_scores[index];
    a = - m_scores[index] + (m_scores[index - 1] + m_scores[index + 1]) / 2.0;
    b = (m_scores[index + 1] - m_scores[index - 1]) / 2.0;


//    std::cout << "a = " <<a <<", b = " <<b << ", c = " << c <<std::endl;
//    std::cout << "m_angles[index] = " << m_angles[index] << ", det = " <<  b/ (a * 2.0) * m_angleDet << std::endl;

    angle = m_angles[index] - b/ (a * 2.0) * m_angleDet;
    score = a * angle * angle + b * angle + c;
    return true;
}

bool TemplateMatch::setModelByAngleCount(const cv::Mat &model, double angleMin, double angleMax, size_t angleCount)
{
    cv::Mat mask = cv::Mat(model.rows, model.cols, CV_8U, 255);
    return setModelByAngleCount(model, mask, angleMin, angleMax, angleCount);
}

bool TemplateMatch::setModelByAngleCount(const cv::Mat &model, const cv::Mat &mask, double angleMin, double angleMax, size_t angleCount)
{
    if(angleCount < 1 || angleCount > 360 * 5 + 1) return false;
    if(angleMax < angleMin) return false;

    m_angleMin = angleMin;
    m_angleCount = angleCount;
    m_angleDet = (angleCount == 1) ? 0 : (angleMax - angleMin) / (angleCount - 1);

    return buildModel(model, mask);

}

bool TemplateMatch::setModelByDetAngle(const cv::Mat &model, double angleMin, double angleMax, double detAngle)
{
    cv::Mat mask = cv::Mat(model.rows, model.cols, CV_8U, 255);
    return setModelByDetAngle(model, mask, angleMin, angleMax, detAngle);
}

bool TemplateMatch::buildModel(const cv::Mat &model, const cv::Mat &mask)
{
    if(model.empty()) return false;
    if(model.size() != mask.size()) return false;

    m_angleMax = m_angleMin + (m_angleCount - 1) * m_angleDet;

    m_rotatedModels.clear();
    m_rotatedMasks.clear();
    m_angles.clear();
    m_scores.clear();

    m_model = model.clone();
    m_mask = mask.clone();
    m_modelSize.height = model.rows;
    m_modelSize.width = model.cols;
    for(size_t i = 0; i < m_angleCount; i++)
    {
        double angle = m_angleMin + i * m_angleDet;
        m_angles.push_back(angle);
        m_scores.push_back(0.0);
        cv::Mat rotatedModel, rotatedMask;
        switch (m_sizeMode) {
        case MODEL_SIZE_MODE::FIX_MODEL_SIZE:
            imageRotateFixSize(model, angle, rotatedModel, rotatedMask);
            break;
        case MODEL_SIZE_MODE::AUTO_MODEL_SIZE:
            imageRotateAutoSize(model, angle, rotatedModel, rotatedMask);
            break;
        case MODEL_SIZE_MODE::FULL_MODEL_SIZE:
            imageRotateFullSize(model, angle, rotatedModel, rotatedMask);
            break;
        }

        if(rotatedModel.rows > m_modelSize.height) m_modelSize.height = rotatedModel.rows;
        if(rotatedModel.cols > m_modelSize.width) m_modelSize.width = rotatedModel.cols;
        m_rotatedModels.push_back(rotatedModel);
        m_rotatedMasks.push_back(rotatedMask);
    }
    return true;
}

bool TemplateMatch::setModelByDetAngle(const cv::Mat &model, const cv::Mat &mask, double angleMin, double angleMax, double detAngle)
{
    if(detAngle <= 0) return false;
    if(angleMax < angleMin) return false;

    m_angleMin = angleMin;
    m_angleCount = ( angleMax + 0.01 - angleMin ) / detAngle;
    m_angleDet = detAngle;

    return buildModel(model, mask);
}

bool drawRotatedRect(cv::Mat &image, cv::RotatedRect &rrt, cv::Scalar color, int thickness)
{
    if(image.empty()) return false;
    //绘制旋转矩形
    cv::Point2f vertices[4];
    rrt.points(vertices);
    for (int i = 0; i < 4; i++)
    {
        cv::line(image, vertices[i], vertices[(i + 1) % 4], color, thickness);
    }
    cv::circle(image, vertices[1], thickness * 3, color, thickness);
    return true;
}

bool TemplateMatch::singleMatch(const cv::Mat &image, int angleIndex, cv::RotatedRect &rrt, double &score)
{
    if(image.empty()) return false;
    if(m_rotatedModels[angleIndex].empty()) return false;
    if(image.rows < m_rotatedModels[angleIndex].rows || image.cols < m_rotatedModels[angleIndex].cols) return false;

    cv::Rect rect;
    bool status = templateMatch(image, m_rotatedModels[angleIndex], m_rotatedMasks[angleIndex], m_matchMethod, rect, score);
    if(status != true) return false;

    double centerX = rect.x + rect.width / 2.0;
    double centerY = rect.y + rect.height /2.0;
    rrt = cv::RotatedRect(cv::Point2f(centerX, centerY), m_model.size(), -m_angles[angleIndex]);
    return true;
}

bool TemplateMatch::singleMatch(const cv::Mat &image, int angleIndex, cv::Rect &rect, double &score)
{
    if(image.empty()) return false;
    if(m_rotatedModels[angleIndex].empty()) return false;
    if(image.rows < m_rotatedModels[angleIndex].rows || image.cols < m_rotatedModels[angleIndex].cols) return false;

    return templateMatch(image, m_rotatedModels[angleIndex], m_rotatedMasks[angleIndex], m_matchMethod, rect, score);
}

//bool TemplateMatch::rotatedMatch(const cv::Mat &image, cv::RotatedRect &rrt, double &score)
//{
//    if(image.empty()) return false;
//    if(m_model.empty()) return false;
//    if(image.rows < m_modelSize.height || image.cols < m_modelSize.width) return false;

//    double bestScore = -1e6, currentScore = -1e6;
//    switch(m_matchMethod)
//    {
//    case cv::TM_SQDIFF:
//    case cv::TM_SQDIFF_NORMED:
//        bestScore = std::numeric_limits<float>::max();
//         break;
//    default:
//        bestScore = std::numeric_limits<float>::min();
//    }

//    int bestIndex = 0;
//    cv::Rect bestRect, rect;
//    for(size_t index = 0; index < m_angleCount; index++)
//    {
//        bool status = templateMatch(image, m_rotatedModels[index], m_rotatedMasks[index], m_matchMethod, rect, currentScore);
//        if(status != true) return false;
//        m_scores[index] = currentScore;
//        if(better(bestScore, currentScore, m_matchMethod))
//        {
//            bestScore = currentScore;
//            bestIndex = (int) index;
//            bestRect = rect;
//        }
//        std::cout << "index = " << index << ", angle = " << m_angles[index] << ", score = " << currentScore << std::endl;
//    }

//    score = bestScore; // 这个是原始的计算值，可以用插值做改善
//    double bestAngle = m_angles[bestIndex]; // 这个是原始的计算值，可以用插值做改善
//    double bestX = bestRect.x;
//    double bestY = bestRect.y;
//    if(m_enableSubPix)
//    {
//        angleInterpolate(bestIndex, bestAngle, score);
//    }

//    double centerX = bestX + bestRect.width / 2.0;
//    double centerY = bestY + bestRect.height /2.0;
//    rrt = cv::RotatedRect(cv::Point2f(centerX, centerY), m_model.size(), -bestAngle);
//    return true;
//}
bool TemplateMatch::rotatedMatch(const cv::Mat &image, cv::RotatedRect &rrt, double &score)
{
    cv::Rect rect;
    size_t index;
    bool ret = rotatedMatch(image, rect, index, score);
    if( !ret ) return false;

    double bestAngle = m_angles[index]; // 这个是原始的计算值，可以用插值做改善
    double bestX = rect.x;
    double bestY = rect.y;
    if(m_enableSubPix)
    {
        angleInterpolate(index, bestAngle, score);
    }

    double centerX = bestX + rect.width / 2.0;
    double centerY = bestY + rect.height /2.0;
    rrt = cv::RotatedRect(cv::Point2f(centerX, centerY), m_model.size(), -bestAngle);
    return true;
}

bool TemplateMatch::rotatedMatch(const cv::Mat &image, cv::Rect &rect, size_t &index, double &score)
{
    if(image.empty()) return false;
    if(m_model.empty()) return false;
    if(image.rows < m_modelSize.height || image.cols < m_modelSize.width) return false;

    double bestScore = -1e6, currentScore = -1e6;
    switch(m_matchMethod)
    {
    case cv::TM_SQDIFF:
    case cv::TM_SQDIFF_NORMED:
        bestScore = std::numeric_limits<float>::max();
         break;
    default:
        bestScore = std::numeric_limits<float>::min();
    }

    int bestIndex = 0;
    cv::Rect bestRect;
    for(size_t index = 0; index < m_angleCount; index++)
    {
        bool status = templateMatch(image, m_rotatedModels[index], m_rotatedMasks[index], m_matchMethod, rect, currentScore);
        if(status != true) return false;
        m_scores[index] = currentScore;
        if(better(bestScore, currentScore, m_matchMethod))
        {
            bestScore = currentScore;
            bestIndex = (int) index;
            bestRect = rect;
        }
        std::cout << "index = " << index << ", angle = " << m_angles[index] << ", score = " << currentScore << std::endl;
    }
    rect = bestRect;
    score = bestScore; // 这个是原始的计算值，可以用插值做改善
    index = bestIndex;
    return true;
}

bool TemplateMatch::rectSize(size_t indexMin, size_t indexMax, int &width, int &height)
{
    width = 0;
    height = 0;
    if(indexMax < indexMin) return false;
    if(indexMax >= m_angleCount) indexMax = m_angleCount - 1;
    for(size_t index = indexMax; index <= indexMax; index ++)
    {
        if(m_rotatedModels[index].rows > height) height = m_rotatedModels[index].rows;
        if(m_rotatedModels[index].cols > width) width = m_rotatedModels[index].cols;
    }
    return true;
}

bool TemplateMatch::getModel(size_t angleIndex, cv::Mat &model)
{
    if(angleIndex > m_angleCount - 1) return false;
    model = m_model.clone();
    return true;
}


}








